#include <assert.h>
#include <malloc.h>
#include <string.h>
#include "queue.h"

QUEUE_HANDLE init_queue()
{
    QUEUE_HANDLE handle = malloc(sizeof(*handle));
    QUEUE_ELEM   *elems  = malloc(sizeof(QUEUE_ELEM) * QUEUE_SIZE);
    memset(elems, 0, sizeof(QUEUE_ELEM) * QUEUE_SIZE);
    handle->data      = elems;
    handle->data_size = QUEUE_SIZE;
    handle->head      = 0;
    handle->elements  = 0;
    sem_init(&handle->semaphore, 0, 0);
    pthread_mutex_init(&handle->mutex, NULL);
    return handle;
}

int queue_push(QUEUE_HANDLE handle, QUEUE_ELEM *elem)
{
    assert(handle != NULL);

    if (handle->elements < handle->data_size)
    {
	/* Get mutex lock and push*/
	int idx;
	pthread_mutex_lock(&handle->mutex);
	idx = (handle->head + handle->elements) % handle->data_size;
	memcpy(&handle->data[idx], elem, sizeof(*elem));
	handle->elements++;
	
	/* Unlock mutex */
	pthread_mutex_unlock(&handle->mutex);
    }
    else
    {
	printf("WOOPS!! Need to grow queue size\n");
    }

    /* Post semaphore */
    sem_post(&handle->semaphore);

    return 0;
}

int queue_pop(QUEUE_HANDLE handle, QUEUE_ELEM *elem)
{
    assert(handle != NULL);
    assert(elem   != NULL);

    /* Wait on semaphore and get mutex lock */
    sem_wait(&handle->semaphore);
    pthread_mutex_lock(&handle->mutex);
    
    if (handle->head < 0 || handle->head >= handle->data_size)
	assert(0);

    memcpy(elem, &handle->data[handle->head], sizeof(*elem));
    handle->head = (handle->head + 1) % handle->data_size;
    handle->elements--;

    /* Release mutex lock */
    pthread_mutex_unlock(&handle->mutex);

    return 0;
}
